/*
 * @Author: liu Shihao
 * @Date: 2022-06-12 21:10:37
 * @LastEditors: liu Shihao
 * @LastEditTime: 2022-06-17 09:41:34
 * @FilePath: \code2\bearpi-hm_nano\applications\BearPi\BearPi-HM_Nano\sample\robot\src\robot_main.c
 * @Description: 
 * Copyright (c) 2022 by ${fzu} email: logic_fzu@outlook.com, All Rights Reserved.
 */

#include "robot_main.h"
#include "robot_body.h"
#include "scs15_control.h"
#include "robot_gail.h"

void robot_init(void)
{
    // 串口初始化
   adc_voltage_init();
   scs_uart2_init();
   ros_uart1_init();
   robot_control_task_init();
   printf("init_ok\r\n");
}

static hi_void *robot_control(hi_void *param)
{
     BodyInit();
  for(;;)
  {
    //body.workstate=remote_VK;
    // voltage_get_data();
    // BodyChange();
    //LegChange();
    //ServoSendData();
    SnycWrite(BR_Leg.IDs,3,BR_Leg.bias_position);
    SnycWrite(FR_Leg.IDs,3,FR_Leg.bias_position);
    SnycWrite(BL_Leg.IDs,3,BL_Leg.bias_position);
    SnycWrite(FL_Leg.IDs,3,FL_Leg.bias_position);
    osDelay(1);
  }
}
hi_u32 robot_control_task_init(hi_void)
{
hi_u32 robot_control_id;
    hi_u32 ret;
    hi_task_attr robot_task = {0};
    robot_task.stack_size = (1024*8);
    robot_task.task_prio = (20);
    robot_task.task_name = (hi_char*)"robot_control_core";
    ret = hi_task_create(&robot_control_id, &robot_task, robot_control, HI_NULL);
    if (ret != HI_ERR_SUCCESS) {
        printf("Failed to create robot_control\r\n");
        return HI_ERR_FAILURE;
    }
    return HI_ERR_SUCCESS;
}
